//===========================================================================
/*!
 *  \file PotHF.cc
 *
 *  \brief 
 *  
 *  \author  Nguyen Quoc Chinh
 *  \date    2007-01-25
 */
//===========================================================================

#include "PotHF.h"
#define DEBUG 0

PotHF::~PotHF(){	
	//cout<<" Destroying everything ++++++++ "<<endl;
	#if MEM_STYLE == 1
		ReleaseVars();
	#endif
}

void PotHF::ReleaseVars(){
	#if MEM_STYLE == 1
		FreeArray(Dtmp,nO*3);	
		FreeArray(Dinv,nO*3);	
		
		FreeArray(E);	
		FreeArray(mu);	
		FreeArray(q);	
		
		FreeArray(r,nAtom);	
		FreeArray(r2,nAtom);	
		FreeArray(Scd,nAtom);	
		FreeArray(dScd,nAtom);	
		FreeArray(Sdd,nAtom);	
		FreeArray(dSdd,nAtom);	
				
		FreeArray(T,nO*3);	
		FreeArray(D,nO*3);	
		
		if(dr){
			for(int i=0;i<nAtom;i++) if(dr[i])FreeArray(dr[i],nAtom);
			delete[] dr;
		}
	#endif
}
		
void PotHF::SetDim_(int dim_){
	if(dim_!=dim){		
		Potential::SetDim_(dim_);
		
		nAtom=dim_/3; 		
		nO=nAtom/2;	
		
		//if(nAtom%3 == 2) nO=nAtom/3+1;	else nO=nAtom/3;
			
		//PrintInfo();
		//ReleaseVars();
		//PrintInfo();
		#if MEM_STYLE == 1
			r=AllocateArray(nAtom,nAtom); 
			r2=AllocateArray(nAtom,nAtom);
			Scd=AllocateArray(nAtom,nAtom);
			dScd=AllocateArray(nAtom,nAtom);
			Sdd=AllocateArray(nAtom,nAtom);
			dSdd=AllocateArray(nAtom,nAtom);
			
			E=AllocateArray(nO*3);
			mu=AllocateArray(nO*3);
			q=AllocateArray(nAtom);
					
			T=AllocateArray(nO*3,nO*3);
			D=AllocateArray(nO*3,nO*3);	
			Dtmp=AllocateArray(nO*3,nO*3);
			Dinv=AllocateArray(nO*3,nO*3);
			
			dr=new double**[nAtom];
			for(int i=0;i<nAtom;i++) dr[i]=AllocateArray(nAtom,DGR);
		#endif
	}
}

double  PotHF::evaluate_( double const *_p){
	int l,m;	
	VEC_TO_MAT2(_p,xtemp,nAtom,DGR);	
	double e=0;	
	//cout<<"===================\n";
	e=huy_oss2_energy_improve(xtemp,nAtom,nO,alpha,0);	
	//cout<<"===================\n";
	//e=oss2energyRed(xtemp,nAtom,nO,alpha);
	//cout<<"===================\n";
	
	ConvertUnit(e,HARTREE,unit);
	
	neval++;
	return e;	
}

void PotHF::gradient_(double const* _p, double* _g){
	int l,m;
	double e=0;
	VEC_TO_MAT2(_p,xtemp,nAtom,DGR);	
	
	huy_oss2_energy_grad(xtemp,nAtom,nO,alpha,e,forcetemp); //using Huy's code
		
	//oss2gradient(xtemp,nAtom,nO,alpha,forcetemp); 
			
	MAT2_TO_VEC(forcetemp,_g,nAtom,DGR);
	
	for(l=0;l<nAtom*DGR;l++) ConvertUnit(_g[l],HARTREE,unit);
	
	neval += 4;
}

int PotHF::ReadParam(const char *filename){
	ifstream in(filename);
	if(!in){ cerr<<"\n Cannot read parameter file !\n"; return _FILENOTEXIST;}

	int i;	string s;
	for(i=0;i<NPARAM;i++){
		in>>alpha[i];
		getline(in,s);
	}	
	in.close();
	return 0;
}


int PotHF::ReadParam(const double *alpha_){
	for(int i=0;i<NPARAM;i++){	alpha[i]=alpha_[i];	}	
	return 0;
}


//=================== Huy's OSS2

#define RP(i,n) for(i=0; i<(n); i++)
#define FR(i,a,b) for(i=(a); i<=(b); i++)
#define SQR(x) ((x)*(x))
#define ABS(x) (((x) > 0) ? (x) : (-(x)))

#define rv(i,j,k) (x[(i)][(k)]-x[(j)][(k)])

//wikipedia scale 
//#define RESCALE 0.5291772108
//fortran scale
#define RESCALE 0.529177249

//#define E_H -0.500272784180     
//#define E_O -74.9663616401     

//definition of parameters
#define p_r0 alpha[0]
#define p_theta0 alpha[1]
#define p_a1 alpha[2]
#define p_a2 alpha[3]
#define p_b1 alpha[4]
#define p_b2 alpha[5]
#define p_c1 alpha[6]
#define p_c2 alpha[7]

#define p_h1 alpha[8]
#define p_h2 alpha[9]
#define p_h3 alpha[10]
#define p_h4 alpha[11]
#define p_h5 alpha[12]

#define p_o1 alpha[13]
#define p_o2 alpha[14]
#define p_o3 alpha[15]
#define p_o4 alpha[16]
#define p_o5 alpha[17]
#define p_o6 alpha[18]
#define p_o7 alpha[19]

#define p_k1 alpha[20]
#define p_k2 alpha[21]
#define p_k3 alpha[22]
#define p_k4 alpha[23]
#define p_k5 alpha[24]
#define p_k6 alpha[25]
#define p_k7 alpha[26]
#define p_k8 alpha[27]
#define p_k9 alpha[28]
#define p_k10 alpha[29]
#define p_k11 alpha[30]
#define p_k12 alpha[31]
#define p_k13 alpha[32]
#define p_k14 alpha[33]
#define p_k15 alpha[34]
#define p_k16 alpha[35]

#define p_m1 alpha[36]
#define p_m2 alpha[37]
#define p_m3 alpha[38]

#define p_alpha alpha[39]

double PotHF::huy_oss2_energy_improve(double x[][DGR], int nAtom, int nO, double alpha[],int inverse){
    // all kind of distance: dOO, dHH, dOH
    
	int i, j, k, l;

#if DEBUG >=2
	cout<<" nAtom = "<<nAtom<<" nO = "<<nO<<endl;
#endif

	RP(i, nAtom) q[i] = ((i<nO)? -1 : 1);

	RP(i, nAtom)
	{
		FR(j, i+1, nAtom-1)
		{
			r2[i][j] = 0;
			RP(k,3) r2[i][j] += SQR(x[i][k] - x[j][k]);
			r[i][j] = sqrt(r2[i][j]);
		}
	}

    // <formula 10>
	double VOO = 0;
	RP(i, nO)
	{
		FR(j, i+1, nO-1)
		{
			VOO += p_o1*exp(-p_o2 * r[i][j]);
			VOO += p_o3*exp(-p_o4 * r[i][j]);
			VOO += p_o5*exp(-p_o6 * SQR(r[i][j] - p_o7));

			VOO += exp(-30.0*(r[i][j]-1.4));
		}
	}
    // </formula 10>


	double VHH = 0;
	for(i=nO;i<nAtom;i++)
		for(j=i+1;j<nAtom;j++)
			VHH += exp(-100.0*(r[i][j]-1));

    // <formula 9>
	double h5tmp = pow(1.0-p_h5,2) / (pow(1.0-p_h5,2) + p_h5*p_h5);

	double VOH = 0;

	RP(i, nO)
	{
		FR(j, nO, nAtom-1)
		{
			VOH += p_h1 * pow(1 - h5tmp*exp(-p_h3*(r[i][j]-p_h2))
					- (1-h5tmp)*exp(-p_h4*(r[i][j]-p_h2)), 2) - p_h1;

			VOH += exp(-70.0*(r[i][j]-0.5));
			//cout<<VOH<<" : "<<r[i][j]<<endl;
		}
	}
    // <formula 9>	

    // <formula 11>
	double VHOH = 0;
	
	double r11, r21, r12, r22, t, dt, dt2;
	RP(i, nO)
	{
		FR(j, nO, nAtom-1)
		{
			FR(k, j+1, nAtom-1)
			{
				r11 = r[i][j] - p_r0;
				r21 = r[i][k] - p_r0;
				r12 = SQR(r11);
				r22 = SQR(r21);
				if(fabs(p_m1*(r12+r22))<46){ // improved by chinh
                
				// theta & shifted theta
					t = acos( (r2[i][j] + r2[i][k] - r2[j][k]) / (2.0*r[i][j]*r[i][k]));
					dt = t - p_theta0;
					dt2 = SQR(dt);
                
					double fcutoff = exp(-(p_m1*(r12+r22) +  p_m2*dt2 + p_m3*(r12+r22)*dt2));

					double temp=0;
				
					temp = p_k1+ p_k2*(r11+r21) + p_k3*dt
							+ p_k4*(r12+r22) + p_k5*r11*r21 + p_k6*dt2 + p_k7*(r11+r21)*dt
							+ p_k8*(r12*r11+r22*r21) + p_k9*(r12*r21+r11*r22) + p_k10*dt*dt2 + p_k11*(r12+r22)*dt
							+ p_k12*r11*r21*dt + p_k13*(r11+r21)*dt2
							+ p_k14*(SQR(r12) + SQR(r22)) + p_k15*r12*r22 + p_k16*SQR(dt2);
		
				
					//cout<<i<<" "<<j<<" "<<k<<" "<<fcutoff<<" "<<temp<<endl;
					VHOH+=temp*fcutoff;
				}
			}
		}
	}
    // </formula 11> 

// <formula 1>

	// <part 1: V coulomb>
	double Vq = 0;
	
	RP(i, nAtom)
	{
		FR(j, i+1, nAtom-1)
		{
			Vq += q[i]*q[j] / r[i][j];
		}
	}
	// </part 1: V coulomb>

	// <part 2: ScdOH, ScdOO, SddOO>
	
	// formula 6
	RP(i, nO)
	{
		FR(j, nO, nAtom-1)
		{
			Scd[i][j] = Scd[j][i]= 1 / (r[i][j]*(r2[i][j] + p_a1*exp(-p_a2*r[i][j])));
			
		}
	}

	// formula 7
	RP(i, nO)
	{
		FR(j, i+1, nO-1)
		{
			Scd[i][j] = Scd[j][i]=1 / (r[i][j]*(r2[i][j] + p_b1*exp(-p_b2*r[i][j])));
			
		}
	}

	// formula 8
	RP(i, nO)
	{
		FR(j, i+1, nO-1)
		{
			Sdd[i][j] =  Sdd[j][i]= 1 / (r[i][j]*(r2[i][j] + p_c1*exp(-p_c2*r[i][j])));
		}
	}

	// </part 2: ScdOH, ScdOO, SddOO>

	// <part 3: matrix T & D>
	double rij[3];

	RP(i, 3*nO)
			RP(j, 3*nO) {  T[i][j] = 0; D[i][j] = 0; }

	RP(i, nO)
	{
		FR(j, i+1, nO-1)
		{
			RP(k, 3)
			{
				rij[k] = x[j][k] - x[i][k];
			}
			RP(k, 3)
			{
				RP(l, 3)
				{
					if (k==l)
						T[3*i+k][3*j+l] = 1 - 3*rij[k]*rij[l] / r2[i][j];
					else
						T[3*i+k][3*j+l] = -3*rij[k]*rij[l] / r2[i][j];

					D[3*i+k][3*j+l]  = T[3*i+k][3*j+l] * Sdd[i][j];

					T[3*j+l][3*i+k] = T[3*i+k][3*j+l];
					D[3*j+l][3*i+k] = D[3*i+k][3*j+l];
				}
			}
		}
	}
	// </part 3: matrix T & D>

	// <part 4: matrix E>
	RP(i, nO)
	{
		double tmp[3];
		
		RP(k,3) tmp[k] = 0;
		
		RP(j, nAtom) if(i!=j){
			RP(k, 3)
			{
				rij[k] = x[j][k] - x[i][k];
				tmp[k] += q[j] * Scd[j][i] * rij[k];
			}
		}
	
		RP(j, 3){
			E[3*i+j] = -p_alpha*tmp[j];
		}
	}
	// </part 4: matrix E>


	// <part5: mu>	
	RP(i, 3*nO){
		FR(j,i, 3*nO-1)
		{
			if (i == j)
				Dtmp[i][j] = 1 + p_alpha*D[i][j];
			else
				Dtmp[i][j] = Dtmp[j][i]= p_alpha*D[i][j];
		}
	}
	//============== Here, we need to solve Dtmp*mu = E for mu ==/
#if DEBUG >=4
	cout<<" mu before solving: "; RP(i, 3*nO){ cout << mu[i] << " ";}cout<<endl;	
#endif
	double tol=1e-6,niter,maxiter=200;
	
	switch(inverse){
		case 0: //solve by inversing Dtmp
#if DEBUG >= 4	
			cout<<"\n++++++ Inverse by Gaussian elimination \n";
#endif
			inv(Dtmp, Dinv, 3*nO);
			RP(i, 3*nO){
				mu[i] = 0;
				RP(j, 3*nO)	mu[i] += Dinv[i][j] * E[j];			
			}
			break;
			case 1: //solve by iterative method - Gauss Seidel			
				niter=IR_Gauss_Seidel(3*nO,Dtmp,mu,E,maxiter,tol);
#if DEBUG >= 4
				cout<<"\n++++++ Solved by Iterative method : ";
				cout<<"\n Step = "<<niter<<" tol="<<tol<<endl;
#endif
				break;
		
	}
	
	//checking if needed
#if DEBUG >= 4
	cout<<" mu after solving: "; RP(i, 3*nO) cout <<setprecision(12)<< mu[i] << " ";cout<<endl;
	double residue=0;
	RP(i, 3*nO){		
		double tmp = -E[i];
		RP(j, 3*nO)	tmp += Dtmp[i][j] * mu[j];
			//cout<<setprecision(4)<<Dtmp[i][j]<<" ";
		residue=(fabs(tmp)>residue)?fabs(tmp):residue;	
	}
	cout<<" Tolerance D*mu - E = "<<residue<<endl;
#endif 
	//===================
	
	// cout << endl;
	// </part5: mu>

	// <part 6: 3 Vsd, Vcd, Vdd>
	double Vsd = 0; double Vdd = 0; double Vcd = 0;
	
	RP(i, nO*3) Vsd += mu[i]*mu[i];
	Vsd /= 2*p_alpha;

	RP(i, 3*nO)
	{
		FR(j, i+1, 3*nO-1)
		{
			Vdd += mu[i]*D[i][j]*mu[j];
		}
	}
						
	RP(i, 3*nO){
		Vcd -= mu[i] * E[i] / p_alpha;
	}
	// </part 6: the rest>
	
	// <part 7: Vel & energy>
	double Vel = 0, Ener = 0;
	Vel = (Vdd + Vcd + Vq + Vsd) * RESCALE;
	Ener = Vel + VOH + VHOH + VOO + VHH;

	// </part 7: Vel>

	// </formula 1>
	
	// results
	if(debug_eval>2){
		printf("\n    VOO     VOH     VHOH     Vq     Vsd     Vdd      Vcd      Vel     Vtotal \n"); 
		printf(" %4.5f %4.5f %4.5f %4.5f  %4.5f %4.5f %4.5f %6.5f  %6.5f \n", 
				VOO,VOH,VHOH,Vq,Vsd,Vdd,Vcd,Vel,Ener); 
	}
	

	return Ener;
}

#define rv(i,j,k) (x[(i)][(k)]-x[(j)][(k)])

void PotHF::huy_oss2_energy_grad(const double x[][DGR], int nAtom, int nO, double alpha[], double& energy, double grad[][3]){
	int i, j, k, l, t;
	double pt1, pt2, pt3, dp, dpk;

    // clear the grad matrix
	RP(i, nAtom) RP(j, 3) grad[i][j] = 0;

	// charges
	RP(i, nAtom) q[i] = ((i<nO)? -1 : 1);

	// distance vectors, euclidian distances and their derivations
	RP(i, nAtom){
		FR(j, i+1, nAtom-1)
		{
			r2[i][j] = 0;

			RP(k, 3){				
				r2[i][j] += SQR(rv(i,j,k));
			}
			r[i][j] = sqrt(r2[i][j]);
			r[j][i] = r[i][j];

			//  dij = SQRT(SQR(ri1 - rj1) + SQR(ri2 - rj2) + SQR(ri3 - rj3))'
			// dij'(rik) = 1/(2d) * (2*(rik-rjk)) = (rik - rjk) / d
			RP(k, 3)
			{
				dr[i][j][k] = rv(i,j,k)/r[i][j];
				dr[j][i][k] =-dr[i][j][k];
			}
		}
	}

	// <formula 1>
	// <part 1: V coulomb>
	double Vq = 0;
	double temp;
	
	RP(i, nAtom)
	{
		FR(j, i+1, nAtom-1)
		{
			Vq += q[i]*q[j] / r[i][j];
			RP(k,3)
			{
				temp=(-q[i]*q[j]/r2[i][j])*dr[i][j][k];
				grad[i][k]+= temp;
				grad[j][k]+=-temp;
			}
		}
	}
	// </part 1: V coulomb>

	// <part 2: ScdOH, ScdOO, SddOO>
	// formula 6
	RP(i, nO)
	{
		FR(j, i+1, nAtom-1)
		{
			Scd[i][j] = 1 / (r[i][j]*(r2[i][j] + p_a1*exp(-p_a2*r[i][j])));
			Scd[j][i] = Scd[i][j];
			dScd[i][j]=-SQR(Scd[i][j])*(3.0*r2[i][j] + p_a1*exp(-p_a2*r[i][j])*(1.0-p_a2*r[i][j]));
			dScd[j][i]=dScd[i][j];
		}
	}

	// formula 7
	RP(i, nO)
	{
		FR(j, i+1, nO-1)
		{
			Scd[i][j] = 1 / (r[i][j]*(r2[i][j] + p_b1*exp(-p_b2*r[i][j])));
			Scd[j][i] = Scd[i][j];
			dScd[i][j]=-SQR(Scd[i][j])*(3.0*r2[i][j] + p_b1*exp(-p_b2*r[i][j])*(1.0-p_b2*r[i][j]));
			dScd[j][i]=dScd[i][j];
		}
	}

	// formula 8
	RP(i, nO)
	{
		FR(j, i+1, nO-1)
		{
			Sdd[i][j] = 1 / (r[i][j]*(r2[i][j] + p_c1*exp(-p_c2*r[i][j])));
			Sdd[j][i] = Sdd[i][j];
			dSdd[i][j]=-SQR(Sdd[i][j])*(3.0*r2[i][j] + p_c1*exp(-p_c2*r[i][j])*(1.0-p_c2*r[i][j]));
			dSdd[j][i]=dSdd[i][j];
		}
	}
	// </part 2: ScdOH, ScdOO, SddOO>

	// <part 3: matrix T & D>
	RP(i, 3*nO)
			RP(j, 3*nO) { T[i][j] = 0; D[i][j] = 0; }

	RP(i, 3*nO)
	{
		T[i][i] = 0;
		D[i][i] = 0;
	}

	RP(i, nO)
	{
		FR(j, i+1, nO-1)
		{
			RP(k, 3)
			{
				RP(l, 3)
				{
					if (k==l)
						T[3*i+k][3*j+l] = 1 - 3*rv(j,i,k)*rv(j,i,l) / r2[i][j];
					else
						T[3*i+k][3*j+l] = -3*rv(j,i,k)*rv(j,i,l) / r2[i][j];

					D[3*i+k][3*j+l]  = T[3*i+k][3*j+l] * Sdd[i][j];

					T[3*j+l][3*i+k] = T[3*i+k][3*j+l];
					D[3*j+l][3*i+k] = D[3*i+k][3*j+l];
				}
			}
		}
	}
	// </part 3: matrix T & D>

	// <part 4: matrix E>
	RP(i, nO)
	{
		double tmp[3];
		
		RP(k,3) tmp[k] = 0;
		
		RP(j, nAtom) if(i!=j){
			RP(k, 3){
				tmp[k] += q[j] * Scd[j][i] * rv(j,i,k);
			}
		}
		
		RP(j, 3)
		{
			E[3*i+j] = -p_alpha*tmp[j];
		}
	}
	// </part 4: matrix E>

	// <part5: mu>	
	RP(i, 3*nO)
	{
		RP(j, 3*nO)
		{
			if (i == j)
				Dtmp[i][j] = 1 + p_alpha*D[i][j];
			else
				Dtmp[i][j] = p_alpha*D[i][j];
		}
	}

	inv(Dtmp, Dinv, 3*nO);

	RP(i, 3*nO)
	{
		mu[i] = 0;
		RP(j, 3*nO)
		{
			mu[i] += Dinv[i][j] * E[j];
		}
		//cout << mu[i] << " ";
	}
	// cout << endl;
	// </part5: mu>

	// <part 6: 3 Vsd, Vcd, Vdd>
	double Vsd = 0;
	RP(i, nO*3) Vsd += mu[i]*mu[i];
	Vsd /= 2*p_alpha;


	double Vdd = 0;
	RP(i, 3*nO)
	{
		FR(j, i+1, 3*nO-1)
		{
			Vdd += mu[i]*D[i][j]*mu[j];
		}
	}

	// gradient Vdd'
	// Vdd = mu[i]*T[ij]*mu[j]*Sdd[ij]
	// Vdd'= mu[i]*T'[ij]*mu[j]*Sdd[ij] + mu[i]*T[ij]*mu[j]*S'dr[ij]
	
	RP(i, nO)
	{
		RP(j, nO)
		{
			if(i!=j){
				pt1 = 0;
				RP(k,3) RP(l,3) pt1 += mu[3*i+k]*T[3*i+k][3*j+l]*mu[3*j+l];

				pt2 = 0;
				pt3 = 0;

				RP(k,3)
				{
					pt2 += mu[i*3+k] * dr[i][j][k];
					pt3 += mu[j*3+k] * dr[i][j][k];
				}
				
				RP(t, 3)
				{
					temp = pt1*dSdd[i][j]*dr[i][j][t];
					temp += 3.0*(2.0*dr[i][j][t]*pt2*pt3
							- mu[j*3+t]*pt2
							- mu[i*3+t]*pt3)/r[i][j]*Sdd[i][j];
			
					grad[i][t]+=temp;
				}
			}
		}
	}

	double Vcd = 0;
	RP(i, 3*nO)
	{
		Vcd -= mu[i] * E[i] / p_alpha;
	}

	// gradient Vcd'
	// Vcd = q[i]*mu[i]*rji*Scd[ij]
	//Vcd = q[i]*mu[i]*rji*Scd[ij]	
	
	RP(i, nAtom)
	{
		RP(j, nO)
		{
			if(i!=j){
				
				RP(t,3)
				{
					double tmp = 0;
					RP(k,3) tmp += mu[3*j+k] * rv(i,j,k);

					tmp =q[i]*tmp*dScd[i][j]*dr[i][j][t];
					tmp+=q[i]*mu[3*j+t]*Scd[i][j];

					grad[i][t]+=tmp;
					grad[j][t]-=tmp;
				}
			}
		}
	}
	
	// </part 6: the rest>


	// <part 7: Vel & energy>
	double Vel = 0, Ener = 0;

	Vel = (Vdd + Vcd + Vq + Vsd) * RESCALE;
	RP(i, nAtom) RP(j, 3) grad[i][j] *= RESCALE;
	// </part 7: Vel>

// </formula 1>

    // <formula 9: 2-body interaction OH>
	double h5tmp = SQR(1.0-p_h5) / (SQR(1.0-p_h5) + p_h5*p_h5);
	double VOH = 0;
	
	RP(i, nO)
	{
		FR(j, nO, nAtom-1)
		{
			pt1 = h5tmp*exp(-p_h3*(r[i][j]-p_h2));
			pt2 = (1-h5tmp)*exp(-p_h4*(r[i][j]-p_h2));

			VOH += p_h1 * SQR(1 - pt1 - pt2) - p_h1;
			
			VOH += exp(-70.0*(r[i][j]-0.5));
			
			// pt3 = pt1' + pt2'
			pt3= -p_h3 * pt1 - p_h4*pt2;
			dp = -p_h1 * 2 * (1-pt1-pt2) * pt3;
			
			dp+=-70.0*exp(-70.0*(r[i][j]-0.5)); //prevent unphysically short F-H
			
			RP(k, 3)
			{
				dpk = dp * dr[i][j][k];
				grad[i][k] += dpk;
				grad[j][k] -= dpk;
			}
		}
		
	}
    // <formula 9>

	// <formula 10: 2-body interaction OO>
	double VOO = 0;
	
	RP(i, nO){
		FR(j, i+1, nO-1){
			pt1 = p_o1*exp(-p_o2 * r[i][j]);
			pt2 = p_o3*exp(-p_o4 * r[i][j]);
			pt3 = p_o5*exp(-p_o6 * SQR(r[i][j] - p_o7));

			VOO += pt1 + pt2 + pt3;
			VOO += exp(-30.0*(r[i][j]-1.4));
			

			// dp = pt1' + pt2' + pt3'
			dp = (-p_o2*pt1 - p_o4*pt2 - p_o6*2.0*(r[i][j] - p_o7)*pt3);
			dp+=-30.0*exp(-30.0*(r[i][j]-1.4)); //prevent unphysically short O-O
			
			RP(k, 3)
			{
				dpk = dp * dr[i][j][k];
				grad[i][k] += dpk;
				grad[j][k] -= dpk;
			}
		}
	}
    // </formula 10>

	double VHH = 0;
	
	for(i=nO;i<nAtom;i++)
		for(j=i+1;j<nAtom;j++){ //H-H
			VHH += exp(-100.0*(r[i][j]-1));
			temp=-100.0*exp(-100.0*(r[i][j]-1));
			RP(t, 3)
			{
				temp*=dr[i][j][t];
				grad[i][t]+=  temp;
				grad[j][t]+= -temp;
		}
		}

    // <formula 11>
		double VHOH = 0;
   
		double r11, r21, r12, r22, cost, theta, dt, dt2, fcutoff;

		double dfcutoff_r1=0,dfcutoff_r2=0,dfcutoff_t=0;

		double dtemp_r1=0,dtemp_r2=0,dtemp_t=0;

		double dVHOH_r1=0,dVHOH_r2=0,dVHOH_t=0;
	
		double dt_ri,dt_rj,dt_rk;

		RP(i, nO)
		{
			FR(j, nO, nAtom-1)
			{
				FR(k, j+1, nAtom-1)
				{
				// shifted distance
					r11 = r[i][j] - p_r0;
					r21 = r[i][k] - p_r0;
					r12 = SQR(r11);
					r22 = SQR(r21);
					if(fabs(p_m1*(r12+r22))<46){ // improved by chinh
						
				// theta & shifted theta
						cost = (r2[i][j] + r2[i][k] - r2[j][k]) / (2.0*r[i][j]*r[i][k]);
						theta = acos(cost);
						dt = theta - p_theta0;
						dt2 = SQR(dt);
                
						fcutoff = exp(-(p_m1*(r12+r22) +  p_m2*dt2 + p_m3*(r12+r22)*dt2));

						double temp = p_k1+ p_k2*(r11+r21) + p_k3*dt
									+ p_k4*(r12+r22) + p_k5*r11*r21 + p_k6*dt2 + p_k7*(r11+r21)*dt
									+ p_k8*(r12*r11+r22*r21) + p_k9*(r12*r21+r11*r22) + p_k10*dt*dt2 + p_k11*(r12+r22)*dt
									+ p_k12*r11*r21*dt + p_k13*(r11+r21)*dt2
									+ p_k14*(SQR(r12) + SQR(r22)) + p_k15*r12*r22 + p_k16*SQR(dt2);
	

						VHOH+=temp*fcutoff;


				// the damn derivatives follow :((
				// VHOH' = temp'*fcutoff + fcutoff'*temp

				// fcutoff'
						dfcutoff_r1=-2.0*(p_m1*r11 + p_m3*r11*dt2)*fcutoff;
						dfcutoff_r2=-2.0*(p_m1*r21 + p_m3*r21*dt2)*fcutoff;
						dfcutoff_t =-2.0*(p_m2+p_m3*(r12+r22))*dt*fcutoff;

				// temp'
						dtemp_r1 = p_k2 + p_k4*2.0*r11 + p_k5*r21 + p_k7*dt
								+ p_k8*3.0*r12 + p_k9*(2.0*r11*r21+r22) + p_k11*2.0*r11*dt
								+ p_k12*r21*dt + p_k13*dt2 +p_k14*4.0*r11*r12 + p_k15*2.0*r11*r22;
						dtemp_r2 = p_k2 + p_k4*2.0*r21 + p_k5*r11 + p_k7*dt
								+ p_k8*3.0*r22 + p_k9*(2.0*r21*r11+r12) + p_k11*2.0*r21*dt
								+ p_k12*r11*dt + p_k13*dt2 +p_k14*4.0*r21*r22 + p_k15*2.0*r21*r12;
						dtemp_t  = p_k3 + p_k6*2.0*dt + p_k7*(r11+r21) + p_k10*3.0*dt2 + p_k11*(r12+r22)
								+ p_k12*r11*r21 + p_k13*(r11+r21)*2.0*dt + p_k16*4.0*dt*dt2;

				// VHOH'
						dVHOH_r1 = fcutoff*dtemp_r1 + dfcutoff_r1*temp;
						dVHOH_r2 = fcutoff*dtemp_r2 + dfcutoff_r2*temp;
						dVHOH_t  = fcutoff*dtemp_t  + dfcutoff_t *temp;

				// r11', r21' and theta'
				// r11' = dr[i][j] && r21' = dr[i][k]
						double sint = sqrt(1-SQR(cost));
				
						RP(t, 3)
						{
							dt_rj = (-1.0/sint) * (cost*dr[i][j][t] - dr[i][k][t])/r[i][j];
							dt_rk = (-1.0/sint) * (cost*dr[i][k][t] - dr[i][j][t])/r[i][k];
							dt_ri = -(dt_rj + dt_rk);
																		
							grad[i][t]+= dVHOH_r1*dr[i][j][t] + dVHOH_r2*dr[i][k][t] + dVHOH_t * dt_ri;
							grad[j][t]+= dVHOH_r1*dr[j][i][t]  						 + dVHOH_t * dt_rj;
							grad[k][t]+=                        dVHOH_r2*dr[k][i][t] + dVHOH_t * dt_rk;
						}
					}


				}
			}
		}
    // </formula 11>	

		energy = Vel + VOH + VHOH + VOO + VHH;

	/*
	if(debug_eval>2){
		printf("\n    VOO     VOH     VHOH     Vq     Vsd     Vdd      Vcd      Vel     Vtotal \n"); \
		printf(" %4.5f %4.5f %4.5f %4.5f  %4.5f %4.5f %4.5f %6.5f  %6.5f \n", \
		VOO,VOH,VHOH,Vq,Vsd,Vdd,Vcd,Vel,Ener); 
	}
		*/
	
}

#if MEM_STYLE == 1
void PotHF::inv(double **A, double **Ainv, int N)
#elif MEM_STYLE == 2
void PotHF::inv(double A[][SIZE_MAX], double Ainv[][SIZE_MAX], int N)
#endif
{
	int i, j, k;
	// for inversion
	//double b[SIZE_MAX][SIZE_MAX];
	//double scale[SIZE_MAX];
	//int tidx[SIZE_MAX];
	
	double **b=AllocateArray(N,N);
	double *scale=AllocateArray(N);
	int *tidx=new int[N];
	
	
	
	//* Matrix b is initialized to the identity matrix

	RP(i, N) RP(j, N) b[i][j] = 0.0;
	RP(i, N){
		b[i][i] = 1.0;
	}

	//* Set scale factor, scale(i) = max( |a(i,j)| ), for each row
	RP(i, N) {
		tidx[i] = i;			  // Initialize row tidx list
		double scalemax = 0.;
		RP(j, N)
				if (scalemax < ABS(A[i][j])) scalemax = ABS(A[i][j]);
		
		scale[i] = scalemax;
	}

	//* Loop over rows k = 0, ..., (N-2)
	int signDet = 1;
	RP(k, N-1) {
		//* Select pivot row from max( |a(j,k)/s(j)| )
		double ratiomax = 0.0;
		int jPivot = k;

		FR(i, k, N-1){
			double ratio = ABS(A[tidx[i]][k])/scale[tidx[i]];
			if( ratio > ratiomax ) {
				jPivot=i;
				ratiomax = ratio;
			}
		}

		//* Perform pivoting using row tidx list
		int tidxJ = tidx[k];

		if( jPivot != k ) {	          // Pivot
			tidxJ = tidx[jPivot];
			tidx[jPivot] = tidx[k];   // Swap tidx jPivot and k
			tidx[k] = tidxJ;
			signDet *= -1;			  // Flip sign of determinant
		}

		//* Perform forward elimination
		FR(i, k+1, N-1) {
			double coeff = A[tidx[i]][k]/A[tidxJ][k];
			FR(j, k+1, N-1)
					A[tidx[i]][j] -= coeff*A[tidxJ][j];

			A[tidx[i]][k] = coeff;

			RP(j, N)
					b[tidx[i]][j] -= A[tidx[i]][k]*b[tidxJ][j];
		}
	}

	//* Compute determinant as product of diagonal elements
	double determ = signDet;	   // Sign of determinant
	RP(i, N) determ *= A[tidx[i]][i];

	//* Perform backsubstitution
	RP(k, N) {
		Ainv[N-1][k] = b[tidx[N-1]][k]/A[tidx[N-1]][N-1];

		for(i=N-2; i>=0; i--) {
			double sum = b[tidx[i]][k];

			FR(j, i+1, N-1)
					sum -= A[tidx[i]][j]*Ainv[j][k];

			Ainv[i][k] = sum/A[tidx[i]][i];
		}
	}
	
	FreeArray(b,N);
	FreeArray(scale);
	delete[] tidx;
}


// Iterative Refinement - Gauss-Seidel Algorithm
// The return value indicates convergence within max_iter (input)
// iterations (0), or no convergence within max_iter iterations (1).
//
// Upon successful return, output arguments have the following values:
//  
//        x  --  approximate solution to Ax = b
// max_iter  --  the number of iterations performed before the
//               tolerance was reached
//      tol  --  the residual after the final iteration
//  
//*****************************************************************
#if MEM_STYLE == 1
int PotHF::IR_Gauss_Seidel(int n,double **A_data,double xk[], const double b[],int max_iter, double &tol)
#elif MEM_STYLE == 2
int PotHF::IR_Gauss_Seidel(int n,double A_data[][SIZE_MAX],double xk[], const double b[],int max_iter, double &tol)
#endif
{
	int i,j;
	double  *xk1,res2=0,tmp,res1=0;
	xk1=new double[n];	
	
	//for(i=0;i<n;i++) xk1[i]=xk[i];	
	for(int iter=0;iter<max_iter;iter++){
		for(i=0;i<n;i++){
			xk1[i]=b[i]; 
			for(j=0;j<i;j++) xk1[i] -=A_data[i][j]*xk1[j];
			for(j=i+1;j<n;j++) xk1[i] -=A_data[i][j]*xk[j];
			xk1[i] /=A_data[i][i];			
		}
		for(res1=0,i=0;i<n;i++){ 
			//tmp=-b[i];for(j=0;j<n;j++) tmp+=A_data[i][j]*xk1[j];tmp=fabs(tmp);
			tmp=fabs(xk1[i]-xk[i]);
			res1=FMAX(tmp,res1);
			xk[i]=xk1[i];
		} 
		//cout<<"Step = "<<iter<<" tol = "<<res2<<" deltaX="<<res1<<endl;		
		if ( res1<= tol){ tol=res1;return iter;}
	}
	tol=res1;	
	
	delete[] xk1;
	return -1;
}



/*
void LinearSolver_LU(int N,double A_data[],double x0[],double b_data[]){
	gsl_matrix_view m = gsl_matrix_view_array (A_data, N, N);
	gsl_vector_view b = gsl_vector_view_array (b_data, N);
	gsl_vector *x = gsl_vector_calloc(N);	
	int s;	
	//printf ("m = \n");gsl_matrix_fprintf (stdout, &m.matrix,"%g");

	gsl_permutation * p = gsl_permutation_alloc (N);
	gsl_linalg_LU_decomp (&m.matrix, p, &s);
	gsl_linalg_LU_solve (&m.matrix, p, &b.vector, x);
	
	for(int i=0;i<N;i++) x0[i]=gsl_vector_get(x,i);
	//printf ("x = \n");gsl_vector_fprintf (stdout, x, "%g");	
	gsl_permutation_free (p);
	gsl_vector_free (x);
}
*/
